/*
 * Copyright (c) 2021  Haowei Wen <yushijinhun@gmail.com>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include "apriltag.hpp"

#include <apriltag/apriltag.h>
#include <apriltag/tag16h5.h>
#include <apriltag/tag25h9.h>
#include <apriltag/tag36h11.h>
#include <apriltag/tagCircle21h7.h>
#include <apriltag/tagCircle49h12.h>
#include <apriltag/tagCustom48h12.h>
#include <apriltag/tagStandard41h12.h>
#include <apriltag/tagStandard52h13.h>

namespace apriltag {

class Detector::Impl {
public:
  Family family;
  apriltag_family_t *tf;
  apriltag_detector_t *td;

  Impl(const Config &cfg) : family(cfg.family) {

    switch (cfg.family) {
    case Family::tag36h11:
      tf = tag36h11_create();
      destroy_func = &tag36h11_destroy;
      break;
    case Family::tag25h9:
      tf = tag25h9_create();
      destroy_func = &tag25h9_destroy;
      break;
    case Family::tag16h5:
      tf = tag16h5_create();
      destroy_func = &tag16h5_destroy;
      break;
    case Family::tagCircle21h7:
      tf = tagCircle21h7_create();
      destroy_func = &tagCircle21h7_destroy;
      break;
    case Family::tagCircle49h12:
      tf = tagCircle49h12_create();
      destroy_func = &tagCircle49h12_destroy;
      break;
    case Family::tagStandard41h12:
      tf = tagStandard41h12_create();
      destroy_func = &tagStandard41h12_destroy;
      break;
    case Family::tagStandard52h13:
      tf = tagStandard52h13_create();
      destroy_func = &tagStandard52h13_destroy;
      break;
    case Family::tagCustom48h12:
      tf = tagCustom48h12_create();
      destroy_func = &tagCustom48h12_destroy;
      break;
    default:
      throw std::invalid_argument("Unknown tag family");
    }

    td = apriltag_detector_create();

    apriltag_detector_add_family(td, tf);

    td->quad_decimate = cfg.quad_decimate;
    td->quad_sigma = cfg.quad_sigma;
    td->nthreads = cfg.nthreads;
    td->debug = 0;
    td->refine_edges = cfg.refine_edges;
  }

  ~Impl() {
    apriltag_detector_destroy(td);
    destroy_func(tf);
  }

private:
  void (*destroy_func)(apriltag_family_t *);
};

Detector::Detector(const Config &cfg) : impl(std::make_unique<Impl>(cfg)){};
Detector::~Detector() = default;

std::vector<Detection> Detector::detect(const cv::Mat &gray) {
  if (gray.type() != CV_8UC1) {
    throw std::invalid_argument("Input image type must be CV_8UC1");
  }

  image_u8_t im = {.width = gray.cols,
                   .height = gray.rows,
                   .stride = gray.cols,
                   .buf = gray.data};
  zarray_t *detections = apriltag_detector_detect(impl->td, &im);
  int detections_size = zarray_size(detections);

  std::vector<Detection> results(detections_size);
  for (int i = 0; i < detections_size; i++) {
    apriltag_detection_t *det;
    zarray_get(detections, i, &det);
    Detection &result = results[i];

    result.family = impl->family;
    result.id = det->id;
    result.hamming = det->hamming;
    result.decision_margin = det->decision_margin;
    memcpy(result.homography_matrix.val, det->H->data, 9 * sizeof(double));
    result.center.x = det->c[0];
    result.center.y = det->c[1];
    result.corners[0] = {det->p[0][0], det->p[0][1]};
    result.corners[1] = {det->p[1][0], det->p[1][1]};
    result.corners[2] = {det->p[2][0], det->p[2][1]};
    result.corners[3] = {det->p[3][0], det->p[3][1]};
  }

  apriltag_detections_destroy(detections);
  return results;
}

}; // namespace apriltag
